#include <iostream>
#include <conio.h>
#include <cmath>

#include <GL/glew.h>
#include <GL/glut.h>


using namespace std;

double const M = 5.97219e24;	//units: kg
double const G = 6.6742e-11;		//units: Nm2/kg2

// void calculate_magnitude(double* Vector, double Magnitude);
void get_initial_conditions (double* Position, double* Velocity, double* Acceleration);
void calculate_inputs(double* U); 
void calculate_new_values(double* R, double* V, double* A, double* U, double* new_R, double* new_V, double* new_A);

int main(){
	

	double r[3] = {}, new_r[3] = {};		
	double v[3] = {}, new_v[3] = {};					
	double a[3] = {}, new_a[3] = {};

	get_initial_conditions (r,v,a);

	cout << "Initial Position:\n" << "\nX:" <<r[0] << "\nY:" <<r[1] <<"\nZ:" <<r[2] << endl << endl;
	cout << "Initial Velocity:\n" << "\nX:" <<v[0] << "\nY:" <<v[1] <<"\nZ:" <<v[2] << endl << endl;
	cout << "Initial Acceleration:\n" << "\nX:" <<a[0] << "\nY:" <<a[1] <<"\nZ:" <<a[2] << endl;

	double values[3] ={};

	calculate_inputs(values);

	for (int i=0; i<10; i++){
		
		calculate_new_values(r,v,a,values,new_r,new_v,new_a);

		cout << "New Position:\n" << "\nX:" <<new_r[0] << "\nY:" <<new_r[1] <<"\nZ:" <<new_r[2] << endl << endl;
		cout << "New Velocity:\n" << "\nX:" <<new_v[0] << "\nY:" <<new_v[1] <<"\nZ:" <<new_v[2] << endl << endl;
		cout << "New Acceleration:\n" << "\nX:" <<new_a[0] << "\nY:" <<new_a[1] <<"\nZ:" <<new_a[2] << endl << endl;
		
		for (int j=0; j<3; j++){
			r[j]=new_r[j];
			v[j]=new_v[j];
			a[j]=new_a[j];
		}
	}


	_getch();
	return 0;

}



void get_initial_conditions (double* Position, double* Velocity, double* Acceleration){

	double Altitude=0;
		cout << "Input the initial altitude of the spacecraft in km: ";
		cin >> Altitude;

		Altitude = (Altitude + 6371) * 1000;		//add radius of Earth, conversion to meter 

		Position[0] = Position [1] = Position [2] = Altitude / sqrt(3);


		cout << "Input the initial velocity of the spacecraft in the km/s: ";

		cout << "\nX Axis: ";
		cin >> Velocity[0];
		Velocity[0] = Velocity[0] * 1000;

		cout << "\nY Axis: ";
		cin >> Velocity[1];
		Velocity[1] = Velocity[1] * 1000;

		cout << "\nZ Axis: ";
		cin >> Velocity[2];
		Velocity[2] = Velocity[2] * 1000;

		cout << "Input the initial acceleration of the spacecraft in the m/s2: ";

		cout << "\nX Axis: ";
		cin >> Acceleration[0];

		cout << "\nY Axis: ";
		cin >> Acceleration[1];
	
		cout << "\nZ Axis: ";
		cin >> Acceleration[2];
		
}


void calculate_new_values(double* R, double* V, double* A, double* U, double* new_R, double* new_V, double* new_A){

	
	//R = vector that holds state variables
	//dt = timestep in seconds
	//N = number of components in vector R
	//Rd = vector that holds the derivatives of the state variables

	R[0];			//Position X
	R[1];			//Position Y
	R[2];			//Position Z
	V[0];			//Velocity X
	V[1];			//Velocity Y
	V[2];			//Velocity Z

	double dt = U[0];
	double m = U[1];
	double F = U[2];
	
	double r2 = R[0]*R[0] + R[1]*R[1] + R[2]*R[2];		//r^2
	double r = sqrt(r2);
	double r3 = r2*r;

	double Formula = ((G*M*m)+(F*r2))/(m*r3);

	for (int i = 0; i<3; i++){
		new_A[i] = (-R[i] * Formula);
		new_V[i] = (new_A[i] * dt) + V[i];
		new_R[i] = (new_V[i] * dt) + R[i];
	}


}

void calculate_inputs(double* U){

	double dt;				// parameter - time step
	double m;				// parameter - Mass of spacecraft
	double F;				// input - thruster on spacecraft
	
	// define parameter

	cout << "\nSpecify the time step in seconds: ";
	cin >> dt;
	
	cout << "\nSpecify the mass of the spacecraft in kg: ";
	cin >> m;

	// define input

	cout << "\nSpecify the thrust in Newton: ";			// I can modify this later to have a user function with respect to t as input
	cin >> F;

	U[0]=dt;
	U[1]=m;
	U[2]=F;


	//

}

